/*
 * Copyright 2010 SRI International
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "ros/console.h"

#include "lesson6/spa_solver.h"

#include <open_karto/Karto.h>

SpaSolver::SpaSolver()
{
}

SpaSolver::~SpaSolver()
{
}

void SpaSolver::Clear()
{
    corrections.clear();
}

const karto::ScanSolver::IdPoseVector &SpaSolver::GetCorrections() const
{
    return corrections;
}

void SpaSolver::Compute()
{
    corrections.clear();

    typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d>> NodeVector;

    ROS_INFO("Calling doSPA for loop closure");
    m_Spa.doSPA(40);
    ROS_INFO("Finished doSPA for loop closure");
    NodeVector nodes = m_Spa.getNodes();
    forEach(NodeVector, &nodes)
    {
        karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
        corrections.push_back(std::make_pair(iter->nodeId, pose));
    }
}

void SpaSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex)
{
    karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
    Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
    m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
}

void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge)
{
    karto::LocalizedRangeScan *pSource = pEdge->GetSource()->GetObject();
    karto::LocalizedRangeScan *pTarget = pEdge->GetTarget()->GetObject();
    karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());

    // 两个位姿间的坐标变换
    karto::Pose2 diff = pLinkInfo->GetPoseDifference();
    Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());

    // 信息矩阵, 协方差矩阵的逆
    karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
    Eigen::Matrix<double, 3, 3> m;
    m(0, 0) = precisionMatrix(0, 0);
    m(0, 1) = m(1, 0) = precisionMatrix(0, 1);
    m(0, 2) = m(2, 0) = precisionMatrix(0, 2);
    m(1, 1) = precisionMatrix(1, 1);
    m(1, 2) = m(2, 1) = precisionMatrix(1, 2);
    m(2, 2) = precisionMatrix(2, 2);

    m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
}
